#include <uwb_sim_core.h>

UwbSim::UwbSim():nh_(""), private_nh_("~"), listener_(buffer_){
    private_nh_.param<std::string>("input_turtlesim_pose_topic_", input_turtlesim_pose_topic_, "");
    private_nh_.param<std::string>("output_uwb_marker_topic_", output_uwb_marker_topic_, "");
    private_nh_.param<std::string>("output_uwb_marker_frame_id_", output_uwb_marker_frame_id_, "");
    private_nh_.param<double>("factor", factor, 0.0);
}

UwbSim::~UwbSim(){}

void UwbSim::run(){
    turtlesim_pose_sub_ = nh_.subscribe<turtlesim::Pose>(input_turtlesim_pose_topic_, 1, &UwbSim::turtlesim_pose_callback, this);
    origin_turtlesim_twist_sub_ = nh_.subscribe<geometry_msgs::Twist>("/turtle1/cmd_vel", 1, &UwbSim::origin_turtlesim_twist_callback, this);
    slow_turtlesim_twist_pub_ = nh_.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel_0", 1);
    uwb_marker_pub_ = nh_.advertise<visualization_msgs::Marker>(output_uwb_marker_topic_, 1);
    uwb_text_marker_pub_ = nh_.advertise<visualization_msgs::Marker>("/uwbsim/uwbtext", 1);
    global_goal_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1);
    ros::Timer timer = nh_.createTimer(ros::Duration(1.0), &UwbSim::process, this, false, true);
    ros::spin();
}

void UwbSim::turtlesim_pose_callback(const turtlesim::Pose::ConstPtr& msg){
    uwb_pose_ = std::make_shared<turtlesim::Pose>(*msg);
    geometry_msgs::PointStamped point_global, point_local_;
    point_global.header.frame_id = "map";
    point_global.point.x = msg->x;
    point_global.point.y = msg->y;

    if(buffer_.canTransform(output_uwb_marker_frame_id_, "map", ros::Time(0))){
        point_local_ = buffer_.transform(point_global, output_uwb_marker_frame_id_);
        visualization_msgs::Marker uwb_marker_;
        uwb_marker_.header.frame_id = output_uwb_marker_frame_id_;
        uwb_marker_.header.stamp = ros::Time().now();
        uwb_marker_.ns = "uwb";
        uwb_marker_.id = 0;
        uwb_marker_.type = uwb_marker_.CYLINDER;
        uwb_marker_.action = uwb_marker_.ADD;
        uwb_marker_.lifetime = ros::Duration(0.05);
        uwb_marker_.pose.position.x = point_local_.point.x;
        uwb_marker_.pose.position.y = point_local_.point.y;
        uwb_marker_.pose.position.z = 0.0;
        uwb_marker_.pose.orientation.x = 0;
        uwb_marker_.pose.orientation.y = 0;
        uwb_marker_.pose.orientation.z = 0;
        uwb_marker_.pose.orientation.w = 1.0;
        uwb_marker_.scale.x = 0.3;
        uwb_marker_.scale.y = 0.3;
        uwb_marker_.scale.z = 0.3;
        uwb_marker_.color.r = 1.0;
        uwb_marker_.color.g = 0.0;
        uwb_marker_.color.b = 0.0;
        uwb_marker_.color.a = 1.0;
        uwb_marker_pub_.publish(uwb_marker_);

        visualization_msgs::Marker uwb_text_marker;
        uwb_text_marker.header.frame_id = output_uwb_marker_frame_id_;
        uwb_text_marker.type = uwb_text_marker.TEXT_VIEW_FACING;
        uwb_text_marker.action = uwb_text_marker.ADD;
        uwb_text_marker.lifetime = ros::Duration(0.2);
        uwb_text_marker.ns = "uwbtext";
        uwb_text_marker.id = 0;
        uwb_text_marker.text = "uwb";
        uwb_text_marker.scale.x = 0.3;
        uwb_text_marker.scale.y = 0.3;
        uwb_text_marker.scale.z = 0.3;
        uwb_text_marker.color.a = 1.0;
        uwb_text_marker.color.r = 1.0;
        uwb_text_marker.color.g = 0.0;
        uwb_text_marker.color.b = 0.0;
        uwb_text_marker.pose.position.x = point_local_.point.x;
        uwb_text_marker.pose.position.y = point_local_.point.y;
        uwb_text_marker_pub_.publish(uwb_text_marker);
    }
}

void UwbSim::origin_turtlesim_twist_callback(const geometry_msgs::Twist::ConstPtr& msg){
    geometry_msgs::Twist slow_twist;
    slow_twist.linear.x = factor*msg->linear.x;
    slow_twist.linear.y = factor*msg->linear.y;
    slow_twist.linear.z = factor*msg->linear.z;

    slow_twist.angular.x = factor*msg->angular.x;
    slow_twist.angular.y = factor*msg->angular.y;
    slow_twist.angular.z = factor*msg->angular.z;
    slow_turtlesim_twist_pub_.publish(slow_twist);
}

 void UwbSim::process(const ros::TimerEvent& e){
    /* 计算车辆当前的位姿 */
    if(uwb_pose_ !=nullptr && buffer_.canTransform("map", "base_link", ros::Time(0))){
        geometry_msgs::TransformStamped base_link_2_map_transform = buffer_.lookupTransform("map", "base_link", ros::Time().now(), ros::Duration(1.0));
        double vehicle_x, vehicle_y;
        vehicle_x = base_link_2_map_transform.transform.translation.x;
        vehicle_y = base_link_2_map_transform.transform.translation.y;
        double distance = sqrt((uwb_pose_->x-vehicle_x)*(uwb_pose_->x-vehicle_x)+(uwb_pose_->y-vehicle_y)*(uwb_pose_->y-vehicle_y));
        if(distance>2.0){
            // 计算目标的yaw角
            double yaw = atan2(uwb_pose_->y-vehicle_y, uwb_pose_->x-vehicle_x);
            double goal_x, goal_y;
            goal_x = vehicle_x + (distance-1.0)*cos(yaw);
            goal_y = vehicle_y + (distance-1.0)*sin(yaw);
            tf::Quaternion q = tf::createQuaternionFromYaw(yaw);
            geometry_msgs::PoseStamped goal;
            goal.header.frame_id = "map";
            goal.pose.position.x = goal_x;
            goal.pose.position.y = goal_y;
            goal.pose.orientation.x = q.getX();
            goal.pose.orientation.y = q.getY();
            goal.pose.orientation.z = q.getZ();
            goal.pose.orientation.w = q.getW();
            global_goal_pose_pub_.publish(goal);
        }   
    }

 }